/**
 * @file /src/qnode.cpp
 * @author Tomonori Yamamoto <tomonori.yamamoto@syntouchllc.com>
 *         (originally written by Daniel Stonier)
 * @date December 02, 2011
 * @brief Ros communication central!
 **/

/*****************************************************************************
** Includes
*****************************************************************************/

#include <string>
#include <sstream>
#include <ros/ros.h>
#include <std_msgs/String.h>
// #include <sr_robot_msgs/Biotac.h>
// #include <sr_robot_msgs/BiotacAll.h>
#include "st_biotac_gui/Biotac.h"
#include "st_biotac_gui/BiotacAll.h"
#include "st_biotac_gui/qnode.hpp"

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace syntouch_biotac_gui {

const int QNode::num_sensing_electrodes_ = 19;
const int QNode::num_biotacs_ = 5;

/*****************************************************************************
** Implementation
*****************************************************************************/

QNode::QNode(int argc, char** argv)
  : init_argc_(argc), init_argv_(argv)
{
  int i, j;
  for (i=0; i<QNode::num_biotacs_; i++)
  {
    data_[i].pac0 = 0;
    data_[i].pac1 = 0;
    data_[i].pdc  = 0;
    data_[i].tac  = 0;
    data_[i].tdc  = 0;
    
    for (j=0; j<QNode::num_sensing_electrodes_; j++)
    {
      data_[i].electrodes[j] = 4095;
    }
  }
}

QNode::~QNode()
{
  if(ros::isStarted())
  {
    ros::shutdown(); // explicitly needed since we use ros::start();
    ros::waitForShutdown();
  }
  wait();
}

//void QNode::callback(const sr_robot_msgs::BiotacAll::ConstPtr& msg)
void QNode::callback(const st_biotac_gui::BiotacAll::ConstPtr& msg)
{
  int i, j;
  for (i=0; i<QNode::num_biotacs_; i++)
  {
    data_[i].pac0 = (int)(msg->tactiles[i].pac0);
    data_[i].pac1 = (int)(msg->tactiles[i].pac1);
    data_[i].pdc  = (int)(msg->tactiles[i].pdc);
    data_[i].tac  = (int)(msg->tactiles[i].tac);
    data_[i].tdc  = (int)(msg->tactiles[i].tdc);
    
    for (j=0; j<QNode::num_sensing_electrodes_; j++)
    {
      data_[i].electrodes[j] = msg->tactiles[i].electrodes[j];
    }
  }
  
  emit dataUpdated();
}

BiotacData* QNode::getData()
{
  return data_;
}

bool QNode::init()
{
  ros::init(init_argc_, init_argv_, "st_biotac_gui");
  if ( ! ros::master::check() ) {
    return false;
  }
  ros::start(); // explicitly needed since our nodehandle is going out of scope.
  ros::NodeHandle n;
  
  // Add your ros communications here.
  chatter_subscriber_ = n.subscribe("/tactile", 2, &QNode::callback, this);
  start();
  return true;
}
  
void QNode::run()
{
  // looping until Ctrl-X pressed
  ros::spin();

  std::cout << "ROS shutdown, proceeding to close the GUI." << std::endl;
  emit rosShutdown(); // used to signal the gui for a shutdown (useful to roslaunch)
}

}  // namespace syntouch_biotac_gui
